#ifndef KALMAN_FILTER_HPP
#define KALMAN_FILTER_HPP

#include "matrix.hpp"

class KalmanFilter {
public:
    KalmanFilter(double R_value, double Q_value);
    
    void update_state_transition_matrix(double dt);
    void predict();
    void extrapolate();
    void update(const Matrix<double>& z);
    
    Matrix<double> get_state() const;
    Matrix<double> get_state_ex() const;

private:
    Matrix<double> x;      // 状态向量 (4x1)
    Matrix<double> x_ex;   // 外推状态向量 (4x1)
    Matrix<double> F;      // 状态转移矩阵 (4x4)
    Matrix<double> H;      // 观测矩阵 (2x4)
    Matrix<double> P;      // 协方差矩阵 (4x4)
    Matrix<double> R;      // 观测噪声协方差 (2x2)
    Matrix<double> Q;      // 过程噪声协方差 (4x4)
};

#endif 